#
#    Ant Colony Game Concept
#    by Evan Border
#
#    Currently Working on: World Generation(in many ways similar to minecraft; having a modular and dynamic world map
#




import direct.directbase.DirectStart
from direct.task import Task
from direct.stdpy import threading
from direct.actor.Actor import Actor
from direct.interval.IntervalGlobal import Sequence
from direct.showbase.DirectObject import DirectObject
from panda3d.core import Point3,NodePath,Vec3,WindowProperties
from pandac.PandaModules import *
from math import pi,sin,cos
from meshGen import MeshGenerator
from time import time
import random,os,sys

global chunkSize
chunkSize = 16
global chunkHeight
chunkHeight = 16
global chunksWide
chunksWide = 64
global collisionLoadRange
collisionLoadRange = 2

global size
size = 2
global vert
vert = 1

global mobID
mobID = 0

class Colony(object,DirectObject):
    def __init__(self):
        self.loadWorld()
        self.loadCollisions()
        self.loadTasks()
        self.loadCamera()
        self.startingUnits()
        self.taskCounter = 0
        #self.taskMgr.add(self.spinCameraTask, "SpinCameraTask")
    def loadCollisions(self):
        base.cTrav = CollisionTraverser()
        global interactionTraverser
        interactionTraverser = CollisionTraverser()
        global interactionQueue
        interactionQueue = CollisionHandlerQueue()
        self.mouseTrav = CollisionTraverser()
        self.pusher = CollisionHandlerPusher()
        self.collisionQueue = CollisionHandlerQueue()
    def loadCamera(self):
        self.camTarget = base.render.attachNewNode("CamTarget")
        
        self.lastChunkX = int(self.camTarget.getX())/chunkSize
        self.lastChunkY = int(self.camTarget.getY())/chunkSize
        self.lastChunkZ = int(self.camTarget.getZ())/chunkHeight
        
        base.camera.reparentTo(self.camTarget)
        base.camera.setPos(10,-10,35)
        base.camera.setHpr(45,-70,0)
        base.disableMouse()
        self.accept("escape", sys.exit)
        self.accept("mouse3", self.rightClick)
        self.accept("mouse1", self.leftClick)
        
        self.myTraverser = CollisionTraverser()        
        self.myHandler = CollisionHandlerQueue()      
        self.pickerNode = CollisionNode('mouseRay') 
        self.pickerNP = base.camera.attachNewNode(self.pickerNode) 
        self.pickerNode.setFromCollideMask(BitMask32.allOn()) 
        self.pickerRay = CollisionRay() 
        self.pickerNode.setIntoCollideMask(BitMask32.allOff()) 
        self.pickerNode.addSolid(self.pickerRay) 
        self.myTraverser.addCollider(self.pickerNP, self.myHandler)
    def testPrint(self):
        print self.taskCounter 
        
    def loadTasks(self):
        base.taskMgr.setupTaskChain("LoadChunks",numThreads = 1,threadPriority = TP_normal)
        base.taskMgr.add(self.moveCameraTask, "cameraMovetask")
        base.taskMgr.add(self.checkLoadedChunks, "checkingLoadedChunks",taskChain = "LoadChunks")
        base.taskMgr.add(self.moveUnits, "unitMoveTask")
        base.taskMgr.add(self.checkInteractions, "checkInteractionsTask")
    def startingUnits(self):
        searching = True
        block = 30
        while searching:
            chunkX,chunkY,chunkZ,x,y,z = coordsToBlocks(0,0,block)
            if chunks[chunkX][chunkY][chunkZ][x][y][z] != -1:

                self.loadUnit(chunkX,chunkY,chunkZ,x,y,z,(0,0,1))
                searching = False
            block -= 1
        
    def checkLoadedChunks(self,task):
        xPos = int(self.camTarget.getX())
        yPos = int(self.camTarget.getY())
        zPos = int(self.camTarget.getZ())
        
        chunkX = xPos/chunkSize
        chunkY = yPos/chunkSize
        chunkZ = zPos/chunkHeight
        if self.lastChunkX != chunkX or self.lastChunkY != chunkY or self.lastChunkZ != chunkZ:
            print "started thread"
            child = self.visibleChunks.getChildren().getPaths()
            for a in range(-(size+1),size+2):
                cX = chunkX+a
                for b in range(-(size+1),size+2):
                    cY = chunkY+b
                    for c in range(-(vert+1),vert+2):
                        cZ = chunkZ+c
                        if chunks[cX][cY][cZ] == -1:
                            self.createChunk(cX,cY,cZ)
            for a in range(-size,size+1):
                cX = chunkX+a
                for b in range(-size,size+1):
                    cY = chunkY+b
                    for c in range(-vert,vert+1):
                        cZ = chunkZ+c                   
                        if self.meshRenders[cX][cY][cZ] == -1:
                            self.loadChunk(cX,cY,cZ)
                        else:
                            if self.meshRenders[cX][cY][cZ] in child:
                                child.remove(self.meshRenders[cX][cY][cZ])
                            else:
                                self.meshRenders[cX][cY][cZ].reparentTo(self.visibleChunks)
            for a in child:
                a.reparentTo(self.discardChunks)
            print "finished Thread"
        self.lastChunkX = chunkX
        self.lastChunkY = chunkY
        self.lastChunkZ = chunkZ
        
        return task.cont
            
    def unCollideChunk(self,i,j):
        self.worldCollisionNodes[i][j].reparentTo(self.unusedCollisions)
    def reCollideChunk(self,i,j):
        self.worldCollisionNodes[i][j].reparentTo(self.collisionGeom)
    
    def click(self):
        if base.mouseWatcherNode.hasMouse(): 
            mpos = base.mouseWatcherNode.getMouse() 
            self.pickerRay.setFromLens(base.camNode, mpos.getX(), mpos.getY())
            self.myTraverser.traverse(base.render)
            if self.myHandler.getNumEntries() > 0: 
                #if we have hit something, sort the hits so that the closest 
                #is first, and highlight that node 
                self.myHandler.sortEntries() 
                i = self.myHandler.getEntry(0)
                return i
            else:
                return None
    def getBlock(self,object):
        rawX,rawY,rawZ = object.getSurfacePoint(base.render)
        rawX = round(rawX,4)
        rawY = round(rawY,4)
        rawZ = round(rawZ,4)
  
        if rawX < 0.0:
            rawX -= 1.0
        if rawY < 0.0:
            rawY -= 1.0
        if rawZ < 0.0:
            rawZ -= 1.0
            
        chunkX,x = divmod(int(rawX),chunkSize)
        chunkY,y = divmod(int(rawY),chunkSize)
        chunkZ,z = divmod(int(rawZ),chunkHeight)
        
        lenX = len(str(rawX).split(".")[1])
        lenY = len(str(rawY).split(".")[1])
        lenZ = len(str(rawZ).split(".")[1])
        
        if lenX == 1:
            if rawX != 0.0:
                norm = (int(-abs(rawX)/rawX),0,0)
            else:
                norm = (-1,0,0)
        elif lenY == 1:
            if rawY != 0.0:
                norm = (0,int(-abs(rawY)/rawY),0)
            else:
                norm = (0,-1,0)
        elif lenZ == 1:
            if rawZ != 0.0:
                norm = (0,0,int(-abs(rawZ)/rawZ))
            else:
                norm = (0,0,-1)
            
        if chunks[chunkX][chunkY][chunkZ][x][y][z] == -1:
            lenX = len(str(rawX).split(".")[1])
            lenY = len(str(rawY).split(".")[1])
            lenZ = len(str(rawZ).split(".")[1])
            if lenX == 1:
                if rawX != 0.0:
                    n = abs(rawX)/rawX
                    rawX -= n
                    norm = (int(n),0,0)
                else:
                    rawX -= 1.0
                    norm = (1,0,0)
            if lenY == 1:
                if rawY != 0.0:
                    n = abs(rawY)/rawY
                    rawY -= n
                    norm = (0,int(n),0)
                else:
                    rawY -= 1.0
                    norm = (0,1,0)
            if lenZ == 1:
                if rawZ != 0.0:
                    n = abs(rawZ)/rawZ
                    rawZ -= n
                    norm = (0,0,int(n))
                else:
                    rawZ -= 1.0
                    norm = (0,0,1)
                        
            chunkX,x = divmod(int(rawX),chunkSize)
            chunkY,y = divmod(int(rawY),chunkSize)
            chunkZ,z = divmod(int(rawZ),chunkHeight)
        return (chunkX,chunkY,chunkZ,x,y,z,norm)

    def rightClick(self):
        object = self.click()
        if object != None:
            if object.getIntoNodePath().getParent() == self.visibleChunks:
                chunkX,chunkY,chunkZ,x,y,z,norm = self.getBlock(object)
          
                self.removeBlock(chunkX,chunkY,chunkZ,x,y,z)
    def leftClick(self):
        object = self.click()
        if object != None:
            if object.getIntoNodePath().getParent() == self.visibleChunks:
                chunkX,chunkY,chunkZ,x,y,z,norm = self.getBlock(object)
                self.loadUnit(chunkX,chunkY,chunkZ,x,y,z,norm)
            else:
                num =  int(object.getIntoNodePath().node().getName())
                self.units[num].moving = 0
          
    def loadWorld(self):
        global chunks
        self.visibleChunks = base.render.attachNewNode("visibleChunks")
        self.discardChunks = NodePath("Chunks to Discard")
        self.visibleCharacters = base.render.attachNewNode("characters")
        self.collisionGeom = base.render.attachNewNode("collisionGeom") 
        self.unusedCollisions = NodePath("Unused Collisions")
        print"Creating Chunk/mesh/collison lists"
        self.worldCollisionNodes = [[[-1 for i in range(chunksWide)] for j in range(chunksWide)]for k in range(chunksWide)]
        chunks = [[[-1 for i in range(chunksWide)] for j in range(chunksWide)]for k in range(chunksWide)]
        self.meshRenders = [[[-1 for i in range(chunksWide)] for j in range(chunksWide)]for k in range(chunksWide)]
        self.units = []
        print"Lists Created"
        print"seeding the chunks"
        self.seedChunk()
        print "chunks Seeded"
        print"Creating Chunk Data"
        total = (2*size)**3
        counter = 1
        for a in range(-(size+1),size+2):
            for b in range(-(size+1),size+2):
                for c in range(-(vert+1),vert+2):
                    self.createChunk(a,b,c)
                    print "chunk created (" + str(counter) + "/" + str(total) + ")"
                    counter += 1
        print"Chunk Creating Done"
        print"Rendering Chunks and Collision surfaces"
        counter = 0
        for a in range(-size,size+1):
            for b in range(-size,size+1):
                for c in range(-vert,vert):
                    self.loadChunk(a,b,c)
                    print "chunk rendered (" + str(counter) + "/" + str(total) + ")"
                    counter+= 1
        print "chunk rendering complete"
        
        
    def seedChunk(self):
        self.varX1Amp = 2*random.random()+1.0
        self.varX2Amp = 2*random.random()+1.0
        self.varY1Amp = 2*random.random()+1.0
        self.varY2Amp = 2*random.random()+1.0
        
        self.varX1Stretch = 1.0/(10.0*random.random()+5.0)
        self.varX2Stretch = 1.0/(10.0*random.random()+5.0)
        self.varY1Stretch = 1.0/(10.0*random.random()+5.0)
        self.varY2Stretch = 1.0/(10.0*random.random()+5.0)
        
        self.varX1Shift = 2.0*pi*random.random()
        self.varX2Shift = 2.0*pi*random.random()
        self.varY1Shift = 2.0*pi*random.random()
        self.varY2Shift = 2.0*pi*random.random()
        
        self.varUpShift = float(chunkHeight/2)
        
        
    def createChunk(self,i,j,k):
        chunk = [[[-1 for derp in range(chunkHeight)] for herp in range(chunkSize)]for plerp in range(chunkSize)]
        for a in range(chunkSize):
            for b in range(chunkSize):
                for c in range(chunkHeight):
                    
                    terrain = (self.varX1Amp*(cos(float(a+chunkSize*i)*self.varX1Stretch + self.varX1Shift))
                               + self.varX2Amp*(cos(float(a+chunkSize*i)*self.varX2Stretch + self.varX2Shift))
                               + self.varY1Amp*(cos(float(b+chunkSize*j)*self.varY1Stretch + self.varY1Shift))
                               + self.varY2Amp*(cos(float(b+chunkSize*j)*self.varY2Stretch + self.varY2Shift)) 
                               + self.varUpShift)
                    testHeight = float(c+k*chunkHeight)
                    if testHeight < terrain - 2.0:
                        chunk[a][b][c] = 1
                    elif testHeight < terrain:
                        chunk[a][b][c] = 0
        chunks[i][j][k] = chunk
        
    def loadChunk(self,i,j,k):
        mesh = MeshGenerator("ChunkGeom " +str(i) + ";"+ str(j)+";"+ str(k))
        
        #ftime = time()
        for a in range(chunkSize):
            for b in range(chunkSize):
                for c in range(chunkHeight):
                    if chunks[i][j][k][a][b][c] != -1:
                        
                        self.testBlockVisibility(i,j,k,a,b,c,mesh)
                    
        if self.meshRenders[i][j][k] != -1:
            self.meshRenders[i][j][k].removeNode()
        self.meshRenders[i][j][k] = self.visibleChunks.attachNewNode(mesh.getGeomNode())
        self.meshRenders[i][j][k].setTexture(loader.loadTexture("Models/tex.png"))

        #print time() - ftime
    def testBlockVisibility(self,i,j,k,x,y,z,mesh):
        id = chunks[i][j][k][x][y][z]
        
        if self.worldCollisionNodes[i][j][k] == -1 :
            self.worldCollisionNodes[i][j][k] = self.unusedCollisions.attachNewNode(CollisionNode("CollGeom " +str(i)+";"+str(j)+";"+str(k)))
            self.worldCollisionNodes[i][j][k].node().setIntoCollideMask(BitMask32.bit(0))
        if x == 0:
            test1 = self.testEmpty(i-1,j,k,(chunkSize-1),y,z)
            test2 = self.testEmpty(i,j,k,x+1,y,z)
        elif x == chunkSize-1:
            test1 = self.testEmpty(i,j,k,x-1,y,z)
            test2 = self.testEmpty(i+1,j,k,0,y,z)
        else:
            test1 = self.testEmpty(i,j,k,x-1,y,z)
            test2 = self.testEmpty(i,j,k,x+1,y,z)
        if test1:
            coll = mesh.makeLeftFace(i*chunkSize+x,j*chunkSize+y,k*chunkHeight+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)
        if test2:
            coll = mesh.makeRightFace(i*chunkSize+x,j*chunkSize+y,k*chunkHeight+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)
        
        if y == 0:
            test3 = self.testEmpty(i,j-1,k,x,(chunkSize-1)-y,z)
            test4 = self.testEmpty(i,j,k,x,y+1,z)
        elif y == chunkSize-1:
            test3 = self.testEmpty(i,j,k,x,y-1,z)
            test4 = self.testEmpty(i,j+1,k,x,(chunkSize-1)-y,z)
        else:
            test3 = self.testEmpty(i,j,k,x,y-1,z)
            test4 = self.testEmpty(i,j,k,x,y+1,z)
        if test3:
            coll = mesh.makeBackFace(i*chunkSize+x,j*chunkSize+y,k*chunkHeight+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)
            
        if test4:
            coll = mesh.makeFrontFace(i*chunkSize+x,j*chunkSize+y,k*chunkHeight+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)    
        
        if z == 0:
            test5 = self.testEmpty(i,j,k-1,x,y,chunkSize -1)
            test6 = self.testEmpty(i,j,k,x,y,z+1)
        elif z == chunkHeight-1:
            test5 = self.testEmpty(i,j,k,x,y,z-1)
            test6 = self.testEmpty(i,j,k+1,x,y,0)
        else:
            test5 = self.testEmpty(i,j,k,x,y,z-1)
            test6 = self.testEmpty(i,j,k,x,y,z+1)
            
        if test5:
            coll = mesh.makeBottomFace(i*chunkSize+x,j*chunkSize+y,k*chunkHeight+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)
            
        if test6:
            coll = mesh.makeTopFace(i*chunkSize+x,j*chunkSize+y,k*chunkSize+z,id)
            self.worldCollisionNodes[i][j][k].node().addSolid(coll)   
        
    def testEmpty(self,i,j,k,x,y,z):
        if chunks[i][j][k] == -1:
            return False
        elif chunks[i][j][k][x][y][z] == -1:
            return True
        else:
            return False
    def testBelow(self,i,j,x,y,z):
        if z == 0:
            return False
        else:
            if chunks[i][j][x][y][z-1] != -1:
                return True
            else:
                return False
    def testAbove(self):
        derp
        
    def removeBlock(self,i,j,k,x,y,z):
        chunks[i][j][k][x][y][z] = -1
        self.testLoadAdjChunks(i,j,k,x,y,z)
        self.loadChunk(i,j,k)
        
    def testLoadAdjChunks(self,i,j,k,x,y,z):
        if x == 0:
            self.loadChunk(i-1,j,k)
        elif x == chunkSize-1:
            self.loadChunk(i+1,j,k)
        if y == 0:
            self.loadChunk(i,j-1,k)
        elif y == chunkSize-1:
            self.loadChunk(i,j+1,k)
        if z == 0:
            self.loadChunk(i,j,k-1)
        elif z == chunkSize-1:
            self.loadChunk(i,j,k+1)
        
    def spinCameraTask(self,task):
        self.camera.setPos(30,20,chunkHeight +5)
        return Task.cont
    def moveCameraTask(self,task):
        t = globalClock.getDt()
        if base.mouseWatcherNode.hasMouse():
            self.taskCounter += 1
            x=base.mouseWatcherNode.getMouseX()
            y=base.mouseWatcherNode.getMouseY()
            maxX = base.win.getXSize()
            maxY = base.win.getYSize()
            t = globalClock.getDt()
            moveThreshold = .8
            moveAmount = 5.0*t
            if x<-moveThreshold:
                self.camTarget.setPos(self.camTarget.getX()-moveAmount,self.camTarget.getY()-moveAmount,self.camTarget.getZ())
            elif x>moveThreshold:
                self.camTarget.setPos(self.camTarget.getX()+moveAmount,self.camTarget.getY()+moveAmount,self.camTarget.getZ())
            if y<-moveThreshold:
                self.camTarget.setPos(self.camTarget.getX()+moveAmount,self.camTarget.getY()-moveAmount,self.camTarget.getZ())
            elif y>moveThreshold:
                self.camTarget.setPos(self.camTarget.getX()-moveAmount,self.camTarget.getY()+moveAmount,self.camTarget.getZ())
        
        return task.cont
    
    def moveUnits(self,task):
        t = globalClock.getDt()
        for a in self.units:
            if a.moving:
                a.moveForward(t)
        return task.cont
    
    def loadUnit(self,chunkX,chunkY,chunkZ,x,y,z,norm):
        unit = WorkerUnit(chunkX,chunkY,chunkZ,x,y,z,norm)
        unit.actor.reparentTo(self.visibleCharacters)
        unit.actor.loop("walk")
        unit.moving = 1
        self.units.append(unit)
        
    def checkInteractions(self,task):
        interactionTraverser.traverse(self.visibleCharacters)
        if interactionQueue.getNumEntries() > 0:
            self.interactUnits()
        return task.cont
    def interactUnits(self):
        print interactionQueue.getEntries()
        
class Unit():
    def __init__(self):
        global mobID
        
        self.id = mobID
        mobID += 1
    def loadActor(self,chunkX,chunkY,chunkZ,x,y,z,norm):
        actorScale = 0.05
        self.actor = Actor(self.modelDir,self.animDir)
        self.actor.setScale(actorScale)
        self.norm = norm
        self.moving = 0
        self.blockLoc = (float(chunkX*chunkSize + x),float(chunkY*chunkSize + y),float(chunkZ*chunkHeight + z))
        x,y,z = chunksToPos(chunkX, chunkY, chunkZ, x, y, z, norm)
        self.actor.setPos(x,y,z)
        h,p,r = self.getHpr(norm)
        self.actor.setHpr(h,p,r)
        self.lastX = self.actor.getX()
        self.lastY = self.actor.getY()
        self.lastZ = self.actor.getZ()
        
        collision = CollisionSphere(0,0,0,15)
        self.collisionPath = self.actor.attachNewNode(CollisionNode(str(self.id)))
        self.collisionPath.node().addSolid(collision)
        #self.collisionPath.node().setIntoCollideMask(GeomNode.getDefaultCollideMask())
        self.collisionPath.node().setCollideMask(BitMask32.bit(1))
        #self.collisionPath.show()
        interactionTraverser.addCollider(self.collisionPath,interactionQueue)
        
    def getHpr(self,norm):
        if norm == (0,0,1):
            hpr = (0,0,0)
            self.direction = (0,-1,0)
        elif norm == (0,0,-1):
            hpr = (0,180,0)
            self.direction = (0,1,0)
        elif norm == (1,0,0):
            hpr = (0,0,90)
            self.direction = (0,-1,0)
        elif norm == (-1,0,0):
            hpr = (0,0,-90)
            self.direction = (0,-1,0)
        elif norm == (0,1,0):
            hpr = (0,-90,0)
            self.direction = (0,0,1)
        elif norm == (0,-1,0):
            hpr = (0,90,0)
            self.direction = (0,0,-1)
        return hpr
    
    def moveForward(self,t):
        speedFactor = .1
        if abs(self.direction[0]) + abs(self.direction[1]) + abs(self.direction[2]) > 1:
            speedFactor = speedFactor*0.7
        realSpeed = self.speed*speedFactor
        self.actor.setPos(self.actor.getX()+self.direction[0]*realSpeed*t,self.actor.getY()+self.direction[1]*realSpeed*t,self.actor.getZ()+self.direction[2]*realSpeed*t)
        self.testOrientation()

        
    def testOrientation(self):
        testVec = False
        testTurn = False
        
        newX = self.actor.getX()
        newY = self.actor.getY()
        newZ = self.actor.getZ()
        
        if self.direction[0] > 0.0:
            xLim = self.blockLoc[0]+1.0
            turnLim = self.blockLoc[0]+0.5
            if newX >= xLim:
                testVec = True
                self.actor.setX(xLim)
                newX = xLim
            elif self.lastX < turnLim and newX >= turnLim:
                testTurn = True
                self.actor.setX(turnLim)
                newX = turnLim
        elif self.direction[0] < 0.0:
            xLim = self.blockLoc[0]
            turnLim = self.blockLoc[0]+0.5
            if newX <= xLim:
                testVec = True
                self.actor.setX(xLim)
                newX = xLim
            elif self.lastX > turnLim and newX <= turnLim:
                testTurn = True
                self.actor.setX(turnLim)
                newX = turnLim
        if self.direction[1] > 0.0:
            yLim = self.blockLoc[1]+1.0
            turnLim = self.blockLoc[1]+0.5
            if newY >= yLim:
                testVec = True
                self.actor.setY(yLim)
                newY = yLim
            elif self.lastY < turnLim and newY >= turnLim:
                testTurn = True
                self.actor.setY(turnLim)
                newY = turnLim
        elif self.direction[1] < 0.0:
            yLim = self.blockLoc[1]
            turnLim = self.blockLoc[1]+0.5
            if newY <= yLim:
                testVec = True
                self.actor.setY(yLim)
                newY = yLim
            elif self.lastY > turnLim and newY <= turnLim:
                testTurn = True
                self.actor.setY(turnLim)
                newY = turnLim
        if self.direction[2] > 0.0:
            zLim = self.blockLoc[2]+1.0
            turnLim = self.blockLoc[2]+0.5
            if newZ >= zLim:
                testVec = True
                self.actor.setZ(zLim)
                newZ = zLim
            elif self.lastZ < turnLim and newZ >= turnLim:
                testTurn = True
                self.actor.setZ(turnLim)
                newZ = turnLim
        elif self.direction[2] < 0.0:
            zLim = self.blockLoc[2]
            turnLim = self.blockLoc[2]+0.5
            if newZ <= zLim:
                testVec = True
                self.actor.setZ(zLim)
                newZ = zLim
            elif self.lastZ > turnLim and newZ <= turnLim:
                testTurn = True
                self.actor.setZ(turnLim)
                newZ = turnLim
        if testVec:
            
            if self.testBlock(self.blockLoc[0]+self.direction[0]+self.norm[0],self.blockLoc[1]+self.direction[1]+self.norm[1],self.blockLoc[2]+self.direction[2]+self.norm[2]):
                self.blockLoc = (self.blockLoc[0]+self.direction[0]+self.norm[0],self.blockLoc[1]+self.direction[1]+self.norm[1],self.blockLoc[2]+self.direction[2]+self.norm[2])
                #self.actor.setP(self.actor.getP()-90)
                newDirec = self.norm
                self.norm = (-self.direction[0],-self.direction[1],-self.direction[2])
                self.direction = newDirec
                self.pointUnit()
            elif self.testBlock(self.blockLoc[0]+self.direction[0],self.blockLoc[1]+self.direction[1],self.blockLoc[2]+self.direction[2]):
                self.blockLoc = (self.blockLoc[0]+self.direction[0],self.blockLoc[1]+self.direction[1],self.blockLoc[2]+self.direction[2])
            else:
                #self.actor.setP(self.actor.getP()+90)
                
                newNorm = self.direction
                self.direction = (-self.norm[0],-self.norm[1],-self.norm[2])
                self.norm = newNorm
                self.pointUnit()
        if testTurn:
            self.randomTurns()
        self.lastX = newX
        self.lastY = newY
        self.lastZ = newZ
        
    def pointUnit(self):
        neg = -1 
        self.actor.lookAt(point = Point3(self.actor.getX()+neg*self.direction[0],self.actor.getY()+neg*self.direction[1],self.actor.getZ()+neg*self.direction[2]),up = Vec3(self.norm[0],self.norm[1],self.norm[2]))
        #self.actor.headsUp(self.actor.getX()+self.norm[0],self.actor.getY()+self.norm[1],self.actor.getZ()+self.norm[2])
    def randomTurns(self):
        turnChance = .2
        r = random.random()
        if r <= turnChance:
            
            turnAmount = random.randint(1,3)*90
            self.setNewDirection(turnAmount)
            
        
    def testBlock(self,rawX,rawY,rawZ):
        chunkX,chunkY,chunkZ,x,y,z = coordsToBlocks(rawX,rawY,rawZ)
        if chunks[chunkX][chunkY][chunkZ][x][y][z] == -1:
            return False
        else:
            return True
        
    def setNewDirection(self,h):
        a = None
        b = None
        counter = 0
        
        for direc in self.norm:
            if direc == 0:
                if a == None:
                    a = counter
                else:
                    b = counter
            counter += 1
            
            
        if a != None and b != None:
            dOne = self.direction[a]
            dTwo = self.direction[b]
            
            numIterations = abs(h/45)
            
            if h < 0:
                for derp in range(numIterations):
                    if dOne == 0 and dTwo == 1:
                        dOne = 1
                    elif dOne == 1 and dTwo == 1:
                        dTwo = 0
                    elif dOne == 1 and dTwo == 0:
                        dTwo = -1
                    elif dOne == 1 and dTwo == -1:
                        dOne = 0
                    elif dOne == 0 and dTwo == -1:
                        dOne = -1
                    elif dOne == -1 and dTwo == -1:
                        dTwo = 0
                    elif dOne == -1 and dTwo == 0:
                        dTwo = 1
                    elif dOne == -1 and dTwo == 1:
                        dOne = 0
            if h > 0:
                for derp in range(numIterations):
                    if dOne == 0 and dTwo == 1:
                        dOne = -1
                    elif dOne == -1 and dTwo == 1:
                        dTwo = 0
                    elif dOne == -1 and dTwo == 0:
                        dTwo = -1
                    elif dOne == -1 and dTwo == -1:
                        dOne = 0
                    elif dOne == 0 and dTwo == -1:
                        dOne = 1
                    elif dOne == 1 and dTwo == -1:
                        dTwo = 0
                    elif dOne == 1 and dTwo == 0:
                        dTwo = 1
                    elif dOne == 1 and dTwo == 1:
                        dOne = 0
                
            if a == 0:
                if b == 1:
                    self.direction = (dOne,dTwo,0)
                else:
                    self.direction = (dOne,0,dTwo)
            else:
                self.direction = (0,dOne,dTwo)
            
            self.pointUnit()
        
            
class WorkerUnit(Unit):
    def __init__(self,chunkX,chunkY,chunkZ,x,y,z,norm):
        Unit.__init__(self)
        self.speed = 10.0
        self.modelDir = ("Models/Actors/slugMan")
        self.animDir = {"walk": "Models/Actors/slugMan-walk"}
        self.loadActor(chunkX,chunkY,chunkZ,x,y,z,norm)

def chunksToPos(chunkX,chunkY,chunkZ,x,y,z,norm):
    x = chunkX*chunkSize + x
    y = chunkY*chunkSize + y
    z = chunkZ*chunkHeight + z
    if norm == (1,0,0):
        xF = float(x)+1.0
        yF = float(y)+0.5
        zF = float(z)+0.5
    elif norm == (-1,0,0):
        xF = float(x)
        yF = float(y)+0.5
        zF = float(z)+0.5
    elif norm == (0,1,0):
        xF = float(x)+0.5
        yF = float(y)+1.0
        zF = float(z)+0.5
    elif norm == (0,-1,0):
        xF = float(x)+0.5
        yF = float(y)
        zF = float(z)+0.5
    elif norm == (0,0,1):
        xF = float(x)+0.5
        yF = float(y)+0.5
        zF = float(z)+1.0
    elif norm == (0,0,-1):
        xF = float(x)+0.5
        yF = float(y)+0.5
        zF = float(z)
    return (xF,yF,zF)

def coordsToBlocks(rawX,rawY,rawZ):
    
    chunkX,x = divmod(int(rawX),chunkSize)
    chunkY,y = divmod(int(rawY),chunkSize)
    chunkZ,z = divmod(int(rawZ),chunkHeight)
    return chunkX,chunkY,chunkZ,x,y,z
        
app = Colony()
run()